/* 
 *  Incub - incubate your baby robot
 *  Copyright (C) 2008 Stanislaw Szymczyk
 *  e-mail: sszymczy@gmail.com   www: http://cogville.cog-lab.com
 *
 *  This file is part of Incub.
 *
 *  Incub is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Incub is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Incub.  If not, see <http://www.gnu.org/licenses/>.
 * 
 */
 
#include "PhysicalSensor.h"
#include "AngularVelocimeter.h"
#include "LinearVelocimeter.h"
#include "TactileSensor.h"
#include "TactileMatrix.h"
#include "Camera.h"
#include "PositionSensor.h"
#include "Physics.h"

using std::vector;

template<>
void PhysicalSensor<AngularVelocimeter>::update() {
	AngularVelocimeter *thiz = dynamic_cast<AngularVelocimeter*>(this);
	const dReal* angularVelocity = dBodyGetAngularVel(parentBodyID);
	thiz->setVelocity(Vector3D(angularVelocity[0], angularVelocity[1], angularVelocity[2]));
}

template<>
void PhysicalSensor<LinearVelocimeter>::update() {
	LinearVelocimeter *thiz = dynamic_cast<LinearVelocimeter*>(this);
	const dReal* linearVelocity = dBodyGetLinearVel(parentBodyID);
	thiz->setVelocity(Vector3D(linearVelocity[0], linearVelocity[1], linearVelocity[2]));
}

template<>
void PhysicalSensor<TactileSensor>::update() {
	TactileSensor *thiz = dynamic_cast<TactileSensor*>(this);
	std::map<Shape*, Vector3D>::iterator force = Physics::forces.find(thiz->getShape());
	if(force != Physics::forces.end()) {
		thiz->setActivation((*force).second.norm());
	} else {
		thiz->setActivation(0.0);
	}
}

template<>
void PhysicalSensor<TactileMatrix>::update() {
	TactileMatrix *thiz = dynamic_cast<TactileMatrix*>(this);
	vector<TactileSensor*>& sensors = thiz->getSensors();
	for(vector<TactileSensor*>::iterator sensor = sensors.begin(); sensor != sensors.end(); sensor++) {
		GenericPhysicalSensor* physicalSensor = dynamic_cast<GenericPhysicalSensor*>(*sensor);
		physicalSensor->update();
	}	
}

template<>
void PhysicalSensor<Camera>::update() {
}

template<>
void PhysicalSensor<PositionSensor>::update() {
}
